public class MyMaze
{

	public static final int MAZESIZE = 4;
	public static final byte GOALX = 3, GOALY = 2, STARTX = 0, STARTY = 0;

	public static final int GOLEFT = 0, GORIGHT = 1;
    public static final int FORWARD = 1, BACKWARD = 0;
	public static final int SOUTH = 0, EAST = 1, NORTH = 2, WEST = 3;
	public static final byte UNVISITED = 0, VISITED = 1, NOWALL = 2, WALL = 3;

	private static int MSize = 2*MAZESIZE+1; // change to 2 later ? WJ: change to 9 by 9 maze
	private static byte maze[][] = new byte[MSize][MSize];
	private static boolean mazeConfirmed;
	private static int goal[] = new int[2];
	public static int position[] = new int[2];
	public static int direction;

    public static int head[][]=new int[100][3];
    public static int pointer = -1;

    public boolean isPathEmpty(){
        return pointer == -1;
    }
    
    public void push(int x,int y,int direction){
        if(pointer+1 < head.length)
        {
        	pointer++;
        	head[pointer][0] = x;
        	head[pointer][1] = y;
        	head[pointer][2] = direction;
        	
        }
    }
    
    public int[] pop(){
        if(isPathEmpty())
            return new int[3];
        	
        return head[pointer--];
    }

	public MyMaze()
	{
		for (int i=0; i < MSize; i++)
			for (int j=0; j < MSize; j++)
			{
				// change init
				if (i == 0 || j == 0 || i == 2*MAZESIZE  || j == 2*MAZESIZE )
					maze[i][j] = WALL;
				else
					maze[i][j] = UNVISITED;
			}
			
		//mazeConfirmed = false;
		// double check this
		// edited by WJ
		goal[0] = (2*GOALX)+1;
		goal[1] = (2*GOALY)+1;
		position[0] = (2*STARTX)+1;
		position[1] = (2*STARTY)+1;
		direction = MyMaze.SOUTH;

		
	}

	public boolean checkgoal()
	{
		return ( position[0] == goal[0] && position[1] == goal[1] );
	}
	
	public boolean checkstart()
	{
		return ( position[0] == (2*STARTX)+1 && position[1] == (2*STARTY)+1 );
	}

	public int findUnexploredNeighbor()
	{
		if(!getWall(MyRobot.LEFT)&&!getVisited(MyRobot.LEFT))
			return MyRobot.LEFT;
		if(!getWall(MyRobot.FRONT)&&!getVisited(MyRobot.FRONT))
			return MyRobot.FRONT;
		if(!getWall(MyRobot.RIGHT)&&!getVisited(MyRobot.RIGHT))
			return MyRobot.RIGHT;
		if(!getWall(MyRobot.BACK)&&!getVisited(MyRobot.BACK))
			return MyRobot.BACK;
			
		return -1;
	}
	
	//TODO: backtracing based on [x,y,direction]
	public int backtrack()
	{
		if(isPathEmpty())
			return -9999;//no path to backtrack
			
		int pos[] = pop();
		
		if(position[0]==pos[0] && position[1]==pos[1])
			pos = pop();
		//pop again if last position is the same as current
		
		int x = position[0] - pos[0];
		int y = position[1] - pos[1];
			 	
		if(direction==MyMaze.SOUTH)
		{
			if(x<0){return MyRobot.LEFT;}
			if(x>0){return MyRobot.RIGHT;}
			if(y>0){return MyRobot.BACK;}
			if(y<0){return MyRobot.FRONT;}
		}
		else if(direction==MyMaze.EAST)
		{
			if(x<0){return MyRobot.BACK;}
			if(x>0){return MyRobot.FRONT;}
			if(y>0){return MyRobot.RIGHT;}
			if(y<0){return MyRobot.LEFT;}
		}
		else if(direction==MyMaze.NORTH)
		{
			if(x<0){return MyRobot.LEFT;}
			if(x>0){return MyRobot.RIGHT;}
			if(y>0){return MyRobot.FRONT;}
			if(y<0){return MyRobot.BACK;}
		}
		else if(direction==MyMaze.WEST)
		{
			if(x<0){return MyRobot.FRONT;}
			if(x>0){return MyRobot.BACK;}
			if(y>0){return MyRobot.LEFT;}
			if(y<0){return MyRobot.RIGHT;}
		}
		
		return -9999;
	}	
		
	public void chgDir(int dir)
	{
		if ( dir == MyMaze.GOLEFT )
			direction = (direction + 1) % 4;
		else if( dir == MyMaze.GORIGHT )
			direction = (direction + 3) % 4;

		// System.out.println(direction);
	}
	
	/* Move the rover forward or backward in the maze
	 * Assumes wall already checked
	 * Moves in steps to allow for efficient traversal
	 * Each step is equivalent to 2 units on the maze */
	 // is steps actually useful?
	public void chgPos(int forward, int step) 
	{
		int move = 2 * step;
		int pos = 1; // translate coordinates by...
	/*	
		for (int i = 0; i < MSize; i++) {
			for (int j = 0; j < MSize; j++)
				System.out.print(maze[j][i]);
			System.out.println();
			
		}*/
		
		//negate the step taken
		if( forward != MyMaze.FORWARD)
		{
			move = -move;
			pos = -pos;
		}
			 
		// TODO add wall verified	
		if (direction == SOUTH)
		{
			// set no wall. this action is redundant later on but
			// this implementation in probably more compact
			maze[position[0]][position[1] + pos] = NOWALL;
			position[1] += move;
		}
		else if (direction == EAST)
		{
			maze[position[0] + pos][position[1]] = NOWALL;
			position[0] += move;
		}
		else if (direction == NORTH)
		{
			maze[position[0]][position[1] - pos] = NOWALL;
			position[1] -= move;
		}
		else if (direction == WEST)
		{
			maze[position[0] - pos][position[1]] = NOWALL;
			position[0] -= move;
		}

		maze[position[0]][position[1]] = VISITED;
		
		if(!isPathEmpty() && position[0]!=head[pointer][0] && position[1]!=head[pointer][1])
			push(position[0],position[1],direction);
		// appending to the navigated path for backtracking purposes
	}

	public boolean getVisited(int dir)
	{
		int pos1 = 2;
		int pos2 = 2; // translate coordinates by...
		
		if (dir == MyRobot.FRONT || dir == MyRobot.BACK){ 
			pos1 = 0;// we can ignore translation of pos 1
			if( dir == MyRobot.BACK )//negate step
				pos2 = -pos2;
		}
		else if (dir == MyRobot.RIGHT || dir == MyRobot.LEFT){
			pos2 = 0;// we can ignore translation of pos 2
			if( dir == MyRobot.LEFT )//negate step
				pos1 = -pos1;
		}
		else 
			return false;
			
		
		// shortened version
		if (direction == SOUTH)
			return (maze[position[0] - pos1][position[1] + pos2] == VISITED);
		else if (direction == EAST)
			return (maze[position[0] + pos2][position[1] + pos1] == VISITED);
		else if (direction == NORTH)
			return (maze[position[0] + pos1][position[1] - pos2] == VISITED);
		else if (direction == WEST)
			return (maze[position[0] - pos2][position[1] - pos1] == VISITED);
	
		
		/*	
		if (dir == MyRobot.FRONT)
		{
			// put elses? not very useful i guess
			if (direction == SOUTH)
				return (maze[position[0]][position[1] + pos ] == VISITED);
			else if (direction == EAST)
				return (maze[position[0] + pos ][position[1]] == VISITED);
			else if (direction == NORTH)
				return (maze[position[0]][position[1] - pos] == VISITED);
			else if (direction == WEST)
				return (maze[position[0] - pos2 ][position[1]] == VISITED);
		}
		else if (dir == MyRobot.BACK)
		{
			if (direction == SOUTH)
				return (maze[position[0]][position[1] - pos] == VISITED);
			if (direction == EAST)
				return (maze[position[0] - pos][position[1]] == VISITED);
			if (direction == NORTH)
				return (maze[position[0]][position[1] + pos] == VISITED);
			if (direction == WEST)
				return (maze[position[0] + pos][position[1]] == VISITED);
		}
		else if (dir == MyRobot.RIGHT)
		{
			if (direction == SOUTH)
				return (maze[position[0] - pos][position[1]] == VISITED);
			if (direction == EAST)
				return (maze[position[0]][position[1] + pos] == VISITED);
			if (direction == NORTH)
				return (maze[position[0] + pos][position[1]] == VISITED);
			if (direction == WEST)
				return (maze[position[0]][position[1] - pos] == VISITED);
		}
		else 
		{
			if (direction == SOUTH)
				return (maze[position[0] + pos][position[1]] == VISITED);
			if (direction == EAST)
				return (maze[position[0]][position[1] - pos] == VISITED);
			if (direction == NORTH)
				return (maze[position[0] - pos][position[1]] == VISITED);
			if (direction == WEST)
				return (maze[position[0]][position[1] + pos] == VISITED);
		}
		*/
		
		return false;
	}

	/*	Place a wall in front of the rover */
	public void putWall()
	{
		if (direction == SOUTH)
			maze[position[0]][position[1] + 1] = WALL;
		else if (direction == EAST)
			maze[position[0] + 1][position[1]] = WALL;
		else if (direction == NORTH)
			maze[position[0]][position[1] - 1] = WALL;
		else if (direction == WEST)
			maze[position[0] - 1][position[1]] = WALL;
	}

	/*	Check if wall is present (has to be sensed) 
	 *	Direction is relative to the rover's direction
	 */
	public boolean getWall(int dir)
	{
		int pos1 = 1;
		int pos2 = 1; // translate coordinates by...
		
		if (dir == MyRobot.FRONT || dir == MyRobot.BACK){ 
			pos1 = 0;// we can ignore translation of pos 1
			if( dir == MyRobot.BACK )//negate step
				pos2 = -pos2;
		}
		else if (dir == MyRobot.RIGHT || dir == MyRobot.LEFT){
			pos2 = 0;// we can ignore translation of pos 2
			if( dir == MyRobot.LEFT )//negate step
				pos1 = -pos1;
		}
		else 
			return false;
			
		
		// shortened version
		if (direction == SOUTH)
			return (maze[position[0] - pos1][position[1] + pos2] == WALL);
		else if (direction == EAST)
			return (maze[position[0] + pos2][position[1] + pos1] == WALL);
		else if (direction == NORTH)
			return (maze[position[0] + pos1][position[1] - pos2] == WALL);
		else if (direction == WEST)
			return (maze[position[0] - pos2][position[1] - pos1] == WALL);
		
		/*	
		if (dir == MyRobot.FRONT)
		{
			if (direction == SOUTH)
				return (maze[position[0]][position[1] + 1] == WALL);
			if (direction == EAST)
				return (maze[position[0] + 1][position[1]] == WALL);
			if (direction == NORTH)
				return (maze[position[0]][position[1] - 1] == WALL);
			if (direction == WEST)
				return (maze[position[0] - 1][position[1]] == WALL);
		}
		else if (dir == MyRobot.BACK)
		{
			if (direction == SOUTH)
				return (maze[position[0]][position[1] - 1] == WALL);
			if (direction == EAST)
				return (maze[position[0] - 1][position[1]] == WALL);
			if (direction == NORTH)
				return (maze[position[0]][position[1] + 1] == WALL);
			if (direction == WEST)
				return (maze[position[0] + 1][position[1]] == WALL);
		}
		else if (dir == MyRobot.RIGHT)
		{
			if (direction == SOUTH)
				return (maze[position[0] - 1][position[1]] == WALL);
			if (direction == EAST)
				return (maze[position[0]][position[1] + 1] == WALL);
			if (direction == NORTH)
				return (maze[position[0] + 1][position[1]] == WALL);
			if (direction == WEST)
				return (maze[position[0]][position[1] - 1] == WALL);
		}
		else 
		{
			if (direction == SOUTH)
				return (maze[position[0] + 1][position[1]] == WALL);
			if (direction == EAST)
				return (maze[position[0]][position[1] - 1] == WALL);
			if (direction == NORTH)
				return (maze[position[0] - 1][position[1]] == WALL);
			if (direction == WEST)
				return (maze[position[0]][position[1] + 1] == WALL);
		}
		*/
		
		return false;
	}

	

	// TODO REDO
	// WJ:ain't sure if it will be redundant
	// CM:yup i think this is redundant.
	/*public static void ConfirmGoal(int type, int x, int y)
	{
		if (type == 0)
		{
			goal[0] = 7 - 2 * x;
			goal[1] = 7 - 2 * y;
		}
		if (type == 1)
		{
			goal[0] = 7 + 2 * x;
			goal[1] = 7 - 2 * y;
		}
		if (type == 2)
		{
			goal[0] = 7 + 2 * x;
			goal[1] = 7 + 2 * y;
		}
		if (type == 3)
		{
			goal[0] = 7 - 2 * x;
			goal[1] = 7 + 2 * y;
		}
	}*/
}
